#ifndef STEREO_CAMERA_RANGING_H
#define STEREO_CAMERA_RANGING_H

#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/ml.hpp>

#include <iostream>
#include <math.h>
#include <string.h>

#define IMAGE_WIDTH 2208
#define IMAGE_HEIGHT 1242

typedef  std::vector<std::vector<cv::Point>> POINT_VECTOR;

struct S_OBJECT_CAMERA_COORDINATE{
    int left_x,left_y;
    int right_x,right_y;
};

struct S_OBJECT_WORLD_COORDINATE{
    double x;
    double y;
    double z;
};

extern cv::Mat g_left_camera_Intrinsic_matrix;
extern cv::Mat g_right_camera_intrinsic_matrix;
extern cv::Mat g_left_camera_distCoeff;
extern cv::Mat g_right_camera_distCoeff;
extern cv::Mat g_rotation_matrix;
extern cv::Mat g_translation_matrix;
extern cv::Mat g_transformation_matrix;
extern cv::Mat g_left_rotation_matrix;
extern cv::Mat g_left_translation_matrix;

class stereo_camera_ranging
{
public:
    stereo_camera_ranging(std::string, std::string);
    virtual ~stereo_camera_ranging();

    S_OBJECT_CAMERA_COORDINATE ImageSegment(cv::Mat, cv::Mat);
    cv::Point3d CoordinateCalculate(S_OBJECT_CAMERA_COORDINATE);

public:
    cv::Point3d m_object_world_coordinate;
    S_OBJECT_CAMERA_COORDINATE m_object_camera_coordiante;
    POINT_VECTOR m_left_image_point, m_right_image_vector;

    cv::Point3d uv2xyz(cv::Point2d uvLeft,cv::Point2d uvRight);
    void findSquares(const cv::Mat&, POINT_VECTOR&);
    void drawSquares(cv::Mat&, const POINT_VECTOR&);

    std::string m_left_image_path, m_right_image_path;
    cv::Mat m_left_image, m_right_image;

};

#endif // STEREO_CAMERA_RANGING_H
